#include "robot.h"
#include "models/command.h"
#include "qendian.h"
#include "warninglogmodel.h"
#include "robotlist.h"
#include "syssettingmodel.h"
#include "models/operatelogmodel.h"

#include <sitckcontrol/stickcontrol.h>

#include <QPainter>
#include <QQuaternion>
#include<math.h>
#include <mainwindow.h>
#include "UrdfWidget.h"



Robot::Robot(QObject *parent)
    : QObject(parent)
{}

Robot::Robot(int nID,QString strName,QString strUrdfName,
             QString strSportIP,
             int nSportPort,
             QString strPerceptionIP,
             int nPerceptionrPort,
             QString strSlamIP,
             int nSlamPort,
             QString strSpeakIP,
             int nSpeakPort,
             QString strZeroIP,
             int nZeroPort,
             QString strTelemetryIP,
             int nTelemetryPort,
             ToolType leftTool,
             ToolType rightTool,
             HeadType headType,
             QString strUser,
             QString strPwd,
             QList<VideoInfo> lsVideo):
    m_sockPerceptionTCP(NULL),
    m_sockPerceptionUDP(NULL),
    m_sockSportUDP(NULL),
    m_sockTelemetryWS(NULL),
    m_sockSpeakWS(NULL),
    m_sockZeroUDP(NULL),
    m_sockSlamUDP(NULL),
    m_nID(nID),
    m_strName(strName),
    m_strUrdfName(strUrdfName),
    m_strSportIP(strSportIP),
    m_nSportPort(nSportPort),
    m_strPerceptionIP(strPerceptionIP),
    m_nPerceptionrPort(nPerceptionrPort),
    m_strSlamIP(strSlamIP),
    m_nSlamPort(nSlamPort),
    m_strSpeakIP(strSpeakIP),
    m_nSpeakPort(nSpeakPort),
    m_strZeroIP(strZeroIP),
    m_nZeroPort(nZeroPort),
    m_strTelemetryIP(strTelemetryIP),
    m_nTelemetryPort(nTelemetryPort),
    m_leftTool(leftTool),
    m_rightTool(rightTool),
    m_headType(headType),
    m_strUser(strUser),
    m_strPassword(strPwd),
    m_lsVideo(lsVideo),
    m_nGlobalCmdID(0),
    m_nUpperCmdID(0),
    m_nAudioFileID(0),
    m_nCurPage(AUTO_PAGE)
{
    //初始化
    memset(&m_SportCmd,0,sizeof(SportCommand));
    m_SportCmd.id = 129;                 //ID
    m_SportCmd.dataNum = sizeof(SportCommand);            //字节数量
    m_SportCmd.ctrMode = 0;            //操控模式 0/1/2
    memset(m_SportCmd.reserve3,0xFF,sizeof(m_SportCmd.reserve3));
    m_leftTip.fill(0,6);
    m_rightTip.fill(0,6);
    m_leftTipExp.fill(0,6);
    m_rightTipExp.fill(0,6);

    m_leftFingerArc.fill(0,6);
    m_leftFingerArcExp.fill(0,6);
    m_leftFingerMoment.fill(0,6);
    m_leftFingerMomentExp.fill(0,6);
    m_leftFingerSpeed.fill(0,6);
    m_leftFingerSpeedExp.fill(0,6);

    m_rightFingerArc.fill(0,6);
    m_rightFingerArcExp.fill(0,6);
    m_rightFingerMoment.fill(0,6);
    m_rightFingerMomentExp.fill(0,6);
    m_rightFingerSpeed.fill(0,6);
    m_rightFingerSpeedExp.fill(0,6);
    connect(SysSettingModel::getInstance(),&SysSettingModel::upperLimbCmdChanged,this,[this](){m_nUpperCmdID = 0; emit upperCmdChanged();});
    connect(SysSettingModel::getInstance(),&SysSettingModel::globalCmdChanged,this,[this](){m_nGlobalCmdID = 0; emit globalCmdChanged();});
    connect(SysSettingModel::getInstance(),&SysSettingModel::audioFileChanged,this,[this](){m_nAudioFileID = 0; emit audioFileChanged();});

}
Robot::~Robot()
{
    disconnectRobot();
}

QString Robot::urdfName(){
    return m_strUrdfName;
}

void Robot::connectRobot()
{
    QString str;
    RobotList::getInstance()->setCurRobot(this);

    if(hasPerception())
    {
        connectPerceptionTCP(str);

    }
    else
    {
        connectSportUDP(str);
    }
    if (auto* p = MainWindow::getInstance()->pUrdfWidget()) {
        p->resetParser();
    }
}

bool Robot::connectPerceptionTCP(QString & strErr)
{
    Q_UNUSED(strErr);
    if(m_sockPerceptionTCP == NULL )
    {
        qDebug()<<"开始连接TCP";
        m_sockPerceptionTCP = new QTcpSocket(this);
        connect(m_sockPerceptionTCP, &QTcpSocket::readyRead, this, &Robot::onPerceptionTCPReadyRead,Qt::UniqueConnection);
        connect(m_sockPerceptionTCP, QOverload<QAbstractSocket::SocketError>::of(&QAbstractSocket::error),this, &Robot::onPerceptionTCPError,Qt::UniqueConnection);
        connect(m_sockPerceptionTCP, &QTcpSocket::connected, this, &Robot::onPerceptionTCPConnect, Qt::UniqueConnection);
        m_sockPerceptionTCP->connectToHost(m_strPerceptionIP, m_nPerceptionrPort);
        QTimer::singleShot(4000,[this](){
            if(m_sockPerceptionTCP&&m_sockPerceptionTCP->state() != QAbstractSocket::ConnectedState)
            {
                qDebug()<<"连接状态检测：m_sockPerceptionTCP->state()="<<m_sockPerceptionTCP->state();
                onPerceptionTCPError(QAbstractSocket::SocketTimeoutError);
            }
        });
    }
    return true;
}

bool Robot::connectPerceptionUDP(QString &strErr)
{
    if(m_sockPerceptionUDP == NULL)
    {
        m_sockPerceptionUDP = new QUdpSocket(this);
        connect(m_sockPerceptionUDP, &QUdpSocket::readyRead, this, &Robot::onPerceptionUDPReadyRead,Qt::UniqueConnection);
        if (!m_sockPerceptionUDP->bind(39393)) {
            strErr = "UDP打开端口39393失败："+m_sockPerceptionUDP->errorString();
            delete m_sockPerceptionUDP;
            m_sockPerceptionUDP = NULL;
            return false;
        }
    }
    return true;
}

bool Robot::connectSportUDP(QString &strErr)
{
    if(m_sockSportUDP == NULL)
    {
        m_sockSportUDP = new QUdpSocket(this);
        connect(m_sockSportUDP, &QUdpSocket::readyRead, this, &Robot::onSportUDPReadyRead,Qt::UniqueConnection);
        if (!m_sockSportUDP->bind(8000))
        {
            strErr = "UDP打开端口8000失败："+m_sockSportUDP->errorString();
            delete m_sockSportUDP;
            m_sockSportUDP = NULL;
            return false;
        }
        m_bRecvHex = false;
        connect(&m_sportSendTimer, &QTimer::timeout, this, &Robot::onSportSendTimer,Qt::UniqueConnection);
        m_sportSendTimer.start(50);
        connect(&m_sportRecvTimer, &QTimer::timeout, this, &Robot::onSportRecvTimer,Qt::UniqueConnection);
        m_sportRecvTimer.start(4000);
    }
    return true;
}

bool Robot::connectSlamUDP(QString &strErr)
{
    if(m_sockSlamUDP == NULL)
    {
        m_sockSlamUDP = new QUdpSocket(this);
        connect(m_sockSlamUDP, &QUdpSocket::readyRead, this, &Robot::onSlamUDPReadyRead,Qt::UniqueConnection);
        if (!m_sockSlamUDP->bind(13579)) {
            strErr = "UDP打开端口13579失败："+m_sockSlamUDP->errorString();
            delete m_sockSlamUDP;
            m_sockSlamUDP = NULL;
            return false;
        }
    }
    getMap();
    connect(&m_reGetMapTimer,&QTimer::timeout,this,&Robot::onReGetMapTimer,Qt::UniqueConnection);
    m_reGetMapTimer.start(4000);
    return true;
}

bool Robot::connectTelemetryWS(QString &strErr)
{
    Q_UNUSED(strErr);
    if(m_sockTelemetryWS == NULL)
    {
        m_sockTelemetryWS = new QWebSocket();
        connect(m_sockTelemetryWS,QOverload<QAbstractSocket::SocketError>::of(&QWebSocket::error),this, &Robot::onTelemetryWSError,Qt::UniqueConnection);
        connect(m_sockTelemetryWS, &QWebSocket::textMessageReceived, this, &Robot::onTelemetryWSReceived);
        m_sockTelemetryWS->open(QUrl(QString("ws://%1:%2").arg(m_strTelemetryIP/*"192.168.1.34"*/).arg(m_nTelemetryPort/*18780*/)));
        QTimer::singleShot(4000,[this](){
            if(m_sockTelemetryWS&&m_sockTelemetryWS->state() != QAbstractSocket::ConnectedState)
            {
                onTelemetryWSError(QAbstractSocket::SocketTimeoutError);
            }
        });
        m_nTelemetryMid = 0;
    }
    return true;
}

void Robot::setStartCmd()
{
    setCmd(152);
}

void Robot::setStopCmd()
{
    setCmd(151);
}

bool Robot::connectZeroUDP(QString &strErr)
{
    Q_UNUSED(strErr);
    if(m_sockZeroUDP == NULL)
    {
        m_sockZeroUDP = new QUdpSocket(this);
    }
    return true;
}

bool Robot::connectSpeakWS(QString &strErr)
{
    Q_UNUSED(strErr);
    if(m_sockSpeakWS == NULL)
    {
        m_sockSpeakWS = new QWebSocket();
        connect(m_sockSpeakWS,QOverload<QAbstractSocket::SocketError>::of(&QWebSocket::error),this, &Robot::onSpeakWSError,Qt::UniqueConnection);
        m_sockSpeakWS->open(QUrl(QString("ws://%1:%2").arg(m_strSpeakIP).arg(m_nSpeakPort)));
        QTimer::singleShot(4000,[this](){
            if(m_sockSpeakWS&&m_sockSpeakWS->state() != QAbstractSocket::ConnectedState)
            {
                onSpeakWSError(QAbstractSocket::SocketTimeoutError);
            }
        });
    }
    return true;
}

void Robot::disConnectPerceptionTCP()
{
    if(m_sockPerceptionTCP)
    {
        disconnect(m_sockPerceptionTCP, NULL, this, NULL);
        m_sockPerceptionTCP->disconnectFromHost();
        m_sockPerceptionTCP->deleteLater();
        m_sockPerceptionTCP = NULL;
    }
}

void Robot::disConnectPerceptionUDP()
{
    if(m_sockPerceptionUDP)
    {
        disconnect(m_sockPerceptionUDP, NULL, this, NULL);
        m_sockPerceptionUDP->close();
        m_sockPerceptionUDP->deleteLater();
        m_sockPerceptionUDP = NULL;
    }
}

void Robot::disConnectSportUDP()
{
    if(m_sockSportUDP)
    {
        disconnect(m_sockSportUDP, NULL, this, NULL);
        m_sockSportUDP->close();
        m_sockSportUDP->deleteLater();
        m_sockSportUDP = NULL;
    }
}

void Robot::disConnectSlamUDP()
{
    if(m_sockSlamUDP)
    {
        disconnect(m_sockSlamUDP, NULL, this, NULL);
        m_sockSlamUDP->close();
        m_sockSlamUDP->deleteLater();
        m_sockSlamUDP = NULL;
    }
}

void Robot::disConnectTelemetryWS()
{
    if(m_sockTelemetryWS)
    {
        disconnect(m_sockTelemetryWS, NULL, this, NULL);
        m_sockTelemetryWS->close();
        m_sockTelemetryWS->deleteLater();
        m_sockTelemetryWS = NULL;
    }
}

void Robot::disConnectZeroUDP()
{

    if(m_sockZeroUDP)
    {
        disconnect(m_sockZeroUDP, NULL, this, NULL);
        m_sockZeroUDP->close();
        m_sockZeroUDP->deleteLater();
        m_sockZeroUDP = NULL;
    }
}

void Robot::disConnectSpeakWS()
{
    if(m_sockSpeakWS)
    {
        disconnect(m_sockSpeakWS, NULL, this, NULL);
        m_sockSpeakWS->close();
        m_sockSpeakWS->deleteLater();
        m_sockSpeakWS = NULL;
    }
}

void Robot::disconnectRobot()
{
    disconnect(StickControl::getInstance(),NULL,this,NULL);
    m_sportSendTimer.stop();
    m_sportRecvTimer.stop();
    memset(&m_SportState,0,sizeof(m_SportState));
    //初始化
    memset(&m_SportCmd,0,sizeof(SportCommand));
    m_SportCmd.id = 129;                 //ID
    m_SportCmd.dataNum = sizeof(SportCommand);            //字节数量
    m_SportCmd.ctrMode = 0;            //操控模式 0/1/2
    memset(m_SportCmd.reserve3,0xFF,sizeof(m_SportCmd.reserve3));
    m_nUpperCmdID = m_nGlobalCmdID = m_nAudioFileID = 0;
    emit upperCmdChanged();
    emit globalCmdChanged();
    emit audioFileChanged();

    disConnectPerceptionTCP();
    disConnectPerceptionUDP();
    disConnectSportUDP();
    disConnectSlamUDP();
    disConnectTelemetryWS();
    disConnectZeroUDP();
    disConnectSpeakWS();

    emit controlStateChanged(false);
    RobotList::getInstance()->setCurRobot(NULL);
}

bool Robot::hasPerception()
{
    return m_strPerceptionIP.length() > 0 && m_nPerceptionrPort > 0;
}

bool Robot::hasSlam()
{
    return m_strSlamIP.length() > 0 && m_nSlamPort > 0;
}

bool Robot::hasSpeak()
{
    return m_strSpeakIP.length() > 0 && m_nSpeakPort > 0;
}

bool Robot::hasZero()
{
    return m_strZeroIP.length() > 0 && m_nZeroPort > 0;
}

bool Robot::hasTelemetry()
{
    return m_strTelemetryIP.length() > 0 && m_nTelemetryPort > 0;
}

bool Robot::requestControl(bool control, QString & strErr)
{
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "request_control";
        dataInfo["value"] = control;

        QJsonObject request;
        request["data_info"] = dataInfo;
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockPerceptionTCP->write(data);
        if(written == data.size())
            return true;
        else
        {
            strErr = m_sockPerceptionTCP->errorString();
            return false;
        }
    }
    else
        return false;
}


bool Robot::startSetJoint(JointType type, int nId, float value)
{
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        QJsonObject request;
        QJsonObject bodyJoint;
        QJsonArray arrData;
        dataInfo["data_type"] = "command_robotstate_manual";
        //根据joint 和id 判断控制的关节
        switch (type){
        case LEFT_TIP_JOINT:
        {
            dataInfo["value"] = "endeffector_joints";
            QJsonObject endeffector_joints;
            QJsonArray left_pose;
            for(int i=0;i<m_leftTip.size();i++)
            {
                if(nId==i)
                    left_pose.append(value);
                else
                    left_pose.append(m_leftTip[i]);
            }
            QJsonArray right_pose;
            for(int i=0;i<m_rightTip.size();i++)
                right_pose.append(m_rightTip[i]);
            // 交换第0、1、2个元素与3、4、5元素
            for (int i = 0; i < 3; ++i) {
                float temp = left_pose[i].toDouble();
                left_pose[i] = left_pose[i + 3];
                left_pose[i + 3] = temp;
                temp = right_pose[i].toDouble();
                right_pose[i] = right_pose[i + 3];
                right_pose[i + 3] = temp;
            }
            endeffector_joints["left_pose"] = left_pose;
            endeffector_joints["right_pose"]=right_pose;
            endeffector_joints["state"] = "start";
            request["data_info"] = dataInfo;
            request["endeffector_joints"] = endeffector_joints;
            break;
        }
        case RIGHT_TIP_JOINT:
        {
            dataInfo["value"] = "endeffector_joints";
            QJsonObject endeffector_joints;
            QJsonArray left_pose;
            for(int i=0;i<m_leftTip.size();i++)
                left_pose.append(m_leftTip[i]);
            QJsonArray right_pose;
            for(int i=0;i<m_rightTip.size();i++)
            {
                if(nId==i)
                    right_pose.append(value);
                else
                    right_pose.append(m_rightTip[i]);
            }
            // 交换第0、1、2个元素与3、4、5元素
            for (int i = 0; i < 3; ++i) {
                float temp = left_pose[i].toDouble();
                left_pose[i] = left_pose[i + 3];
                left_pose[i + 3] = temp;
                temp = right_pose[i].toDouble();
                right_pose[i] = right_pose[i + 3];
                right_pose[i + 3] = temp;
            }
            endeffector_joints["left_pose"]=left_pose;
            endeffector_joints["right_pose"]=right_pose;
            endeffector_joints["state"] = "start";
            request["data_info"] = dataInfo;
            request["endeffector_joints"] = endeffector_joints;
            break;
        }
        case LEFT_TOOL_HAND_JOINT:
        {
            dataInfo["value"] = "joints";
            if(m_leftFingerArc.size()>nId)
            {
                arrData.append(value);//获取元数据加上要修改的值
                arrData.append(0);
                arrData.append(0);
            }
            bodyJoint[getJointByType(type)[nId].strProtocal] = arrData;
            bodyJoint["state"] = "start";
            request["data_info"] = dataInfo;
            request["body_joints"] = bodyJoint;
            break;
        }
        case LEFT_TOOL_GRIPPER_JOINT:
        case LEFT_TOOL_SUCKER_JOINT:
        {
            dataInfo["value"] = "joints";
            if(m_leftFingerArc.size()>nId)
            {
                arrData.append(value);//获取元数据加上要修改的值
                arrData.append(0);
                arrData.append(0);
            }
            bodyJoint[getJointByType(type)[0].strProtocal] = arrData;
            bodyJoint["state"] = "start";
            request["data_info"] = dataInfo;
            request["body_joints"] = bodyJoint;
            break;
        }
        case RIGHT_TOOL_HAND_JOINT:
        {
            dataInfo["value"] = "joints";
            if(m_rightFingerArc.size()>nId)
            {
                arrData.append(value);//获取元数据加上要修改的值
                arrData.append(0);
                arrData.append(0);
            }
            bodyJoint[getJointByType(type)[nId].strProtocal] = arrData;
            bodyJoint["state"] = "start";
            request["data_info"] = dataInfo;
            request["body_joints"] = bodyJoint;
            break;
        }
        case RIGHT_TOOL_GRIPPER_JOINT:
        case RIGHT_TOOL_SUCKER_JOINT:
        {
            dataInfo["value"] = "joints";
            if(m_rightFingerArc.size()>nId)
            {
                arrData.append(value);//获取元数据加上要修改的值
                arrData.append(0);
                arrData.append(0);
            }
            bodyJoint[getJointByType(type)[0].strProtocal] = arrData;
            bodyJoint["state"] = "start";
            request["data_info"] = dataInfo;
            request["body_joints"] = bodyJoint;
            break;
        }
        default:
        {
            QJsonObject bodyJoint;
            QJsonArray arrData;
            dataInfo["value"] = "joints";
            QVector<float> JointsArc =getJointsArc(type);
            if(JointsArc.size()>nId)
            {
                arrData.append(value);//获取元数据加上要修改的值
                arrData.append(0);
                arrData.append(0);
            }
            bodyJoint[getJointByType(type)[nId].strProtocal] = arrData;
            bodyJoint["state"] = "start";
            request["data_info"] = dataInfo;
            request["body_joints"] = bodyJoint;
            break;
        }
        }
        m_setJointCmd = request;
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        // OPERATE_LOG(QString("设置关节位置：%1%2").arg(getJointTypeText(type)).arg()
        //                 .arg(left_rx).arg(left_ry).arg(left_rz).arg(left_x).arg(left_y).arg(left_z).arg(left_grasp)
        //                 .arg(right_rx).arg(right_ry).arg(right_rz).arg(right_x).arg(right_y).arg(right_z).arg(right_grasp));

        qint64 written = m_sockPerceptionTCP->write(data);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::stopSetJoint()
{
    QJsonObject dataInfo = m_setJointCmd["data_info"].toObject();
    if(dataInfo["data_type"].toString() == "command_robotstate_manual")
    {
        if(dataInfo["value"] == "endeffector_joints")
        {
            QJsonObject joints = m_setJointCmd["endeffector_joints"].toObject();
            joints["state"] = "stop";
            m_setJointCmd["endeffector_joints"] = joints;
        }
        else if(dataInfo["value"] == "joints")
        {
            QJsonObject joints = m_setJointCmd["body_joints"].toObject();

            joints["state"] = "stop";
            m_setJointCmd["body_joints"] = joints;
        }
        QByteArray data = QJsonDocument(m_setJointCmd).toJson(QJsonDocument::Compact);
        qint64 written = m_sockPerceptionTCP->write(data);
        return written == data.size();
    }
    return false;
}

bool Robot::startDemonstrate(QString jsonData, int times)
{
    setCmd(152);
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "command_demonstrate";
        dataInfo["value"] = "start";

        QJsonObject request;
        request["data_info"] = dataInfo;
        QJsonParseError err;
        QJsonDocument doc = QJsonDocument::fromJson(jsonData.toUtf8(),&err);
        if(!doc.isEmpty() && err.error == QJsonParseError::NoError)
        {
            QJsonObject demonstrate= doc.object();
            demonstrate["times"] = times;
            request["demonstrate"] = demonstrate;
        }
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockPerceptionTCP->write(data);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::stopDemonstrate()
{
    setCmd(151);
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "command_demonstrate";
        dataInfo["value"] = "stop";

        QJsonObject request;
        request["data_info"] = dataInfo;
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockPerceptionTCP->write(data);
        return written == data.size();
    }
    else
        return false;
    return true;
}

bool Robot::setTurn(float fTurn)
{
    if(m_bJoystickState){
        m_SportCmd.xSpeed = fTurn * 100;
    }else{
        m_SportCmd.turnSpeed = -fTurn * 100;
    }
    return true;
}

bool Robot::setMove(float fX, float fY)
{
    if(m_bJoystickState){
        m_SportCmd.turnSpeed = -fX * 100;
    }else{
        m_SportCmd.xSpeed = fX * 100;
    }
    m_SportCmd.ySpeed = fY * 100;
    return true;
}

bool Robot::setUpperCmd(Command cmd)
{
    if(cmd.strFile.length() > 0)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "command_hdf5";
        QJsonObject value;
        value["hdf5_file"] = cmd.strFile;
        value["play_rate"] = 50.0;
        if(m_nUpperCmdID == cmd.nID)
        {
            value["enable"] = false;
            m_nUpperCmdID = cmd.nParentID;
            cmd = *SysSettingModel::getInstance()->getUpperLimbCmd(m_nUpperCmdID);
        }
        else
        {
            value["enable"] = true;
            m_nUpperCmdID = cmd.nID;
        }
        dataInfo["value"] = value;
        if(m_sockPerceptionTCP)
        {
            QJsonObject request;
            request["data_info"] = dataInfo;
            QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
            m_sockPerceptionTCP->write(data);
        }
    }
    setCmd(cmd.nHexCmd);
    m_nUpperCmdID = cmd.nID;
    emit upperCmdChanged();
    OPERATE_LOG("执行上肢动作："+cmd.strShowText);

    return true;
}

bool Robot::setGlobalCmd(Command cmd)
{
    setCmd(cmd.nHexCmd);
    m_nGlobalCmdID = cmd.nID;
    emit globalCmdChanged();
    OPERATE_LOG("执行全身动作："+cmd.strShowText);
    return true;
}

void Robot::setEnable(bool bEnable)
{
    if(bEnable)
    {
        Command cmd;
        if(SysSettingModel::getInstance()->getCmdByName("上使能",cmd) != CMD_TYPE_NONE)
            setCmd(cmd.nHexCmd);
        QTimer::singleShot(1000,[=](){
            setPage(m_nCurPage);
        });
        OPERATE_LOG("执行全身动作：上使能");
    }
    else
    {
        Command cmd;
        if(SysSettingModel::getInstance()->getCmdByName("下使能",cmd) != CMD_TYPE_NONE)
            setCmd(cmd.nHexCmd);
        m_nUpperCmdID = m_nGlobalCmdID = 0;
        emit upperCmdChanged();
        emit globalCmdChanged();
        OPERATE_LOG("执行全身动作：下使能");
    }
}

void Robot::sentryIndify(int nType)
{
    setSentryOptical(0xA0,0x02,nType);
    OPERATE_LOG("哨兵头-识别");
}

void Robot::sentryTrack(int nVideo)
{
    setSentryOptical(0xA3,nVideo,0x00);
    OPERATE_LOG("哨兵头-跟踪");
}

void Robot::sentryGetResult()
{
    setSentryOptical(0xA4,0x00,0x00);
    OPERATE_LOG("哨兵头-获取识别追踪信息");
}

void Robot::sentryDestance()
{
    setSentryLaser();
    OPERATE_LOG("哨兵头-测距");
}

void Robot::setSentryOptical(int nFunc, int nMode, int nParam)
{
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "command_guard";
        dataInfo["value"] = "optical";

        QJsonObject optical;
        optical["function"] = nFunc;
        optical["mode"] = nMode;
        optical["parameter"] = nParam;


        QJsonObject request;
        request["data_info"] = dataInfo;
        request["optical"] = optical;

        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        m_sockPerceptionTCP->write(data);
    }
}

void Robot::setSentryLaser()
{
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "command_guard";
        dataInfo["value"] = "laser";

        QJsonObject request;
        request["data_info"] = dataInfo;

        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        m_sockPerceptionTCP->write(data);
    }
}

void Robot::setAudioFlie(AudioFile file)
{
    if(m_sockPerceptionTCP)
    {
        bool bEnable = false;
        if(m_nAudioFileID == file.nID)
        {
            bEnable = false;
            m_nAudioFileID = 0;
        }
        else
        {
            bEnable = true;
            m_nAudioFileID = file.nID;
        }
        if(file.strAudioFile.length() > 0)
        {
            QJsonObject dataInfo;
            dataInfo["data_type"] = "command_audio";
            QJsonObject value;
            value["audio_file"] = file.strAudioFile;
            value["enable"] = bEnable;
            dataInfo["value"] = value;
            QJsonObject request;
            request["data_info"] = dataInfo;
            QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
            m_sockPerceptionTCP->write(data);
        }
        if(file.strActionFile.length() > 0)
        {
            QJsonObject dataInfo;
            dataInfo["data_type"] = "command_hdf5";
            QJsonObject value;
            value["hdf5_file"] = file.strActionFile;
            value["play_rate"] = 50.0;
            value["enable"] = bEnable;
            dataInfo["value"] = value;
            QJsonObject request;
            request["data_info"] = dataInfo;
            QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
            m_sockPerceptionTCP->write(data);
        }
        OPERATE_LOG("播放音频："+file.strShowText);
        emit audioFileChanged();
    }
}

bool Robot::getMap()
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "get_map";
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::initPosition(QPointF position, float angle)
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "init_position";
        QJsonObject pos;
        pos["x"] = position.x();
        pos["y"] = position.y();
        request["position"] = pos;
        QJsonObject orientation;
        QQuaternion quaternion = QQuaternion::fromEulerAngles(0,0,angle);
        orientation["w"] = quaternion.scalar();
        orientation["x"] = quaternion.x();
        orientation["y"] = quaternion.y();
        orientation["z"] = quaternion.z();
        request["orientation"] = orientation;
        //QVector3D eulerAngles = quaternion.toEulerAngles();
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        m_ptMapRobot = position;
        m_fMapRobotAngle = angle;
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);

        OPERATE_LOG(QString("SLAM：设置初始位置(%1,%2),角度(%3°)").arg(position.x()).arg(position.y()).arg(angle));
        return written == data.size();
    }
    else
        return false;
}

bool Robot::startPosition()
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "start_position";
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::stopPosition()
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "stop_position";
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::startPointCloud()
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "start_pointcloud";
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::stopPointCloud()
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "stop_pointcloud";
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::startNavigatione(QPointF dest, float angle)
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "start_navigatione";
        QJsonObject pos;
        pos["x"] = dest.x();
        pos["y"] = dest.y();
        request["dest_position"] = pos;
        QJsonObject orientation;
        QQuaternion quaternion = QQuaternion::fromEulerAngles(0,0,angle);
        orientation["w"] = quaternion.scalar();
        orientation["x"] = quaternion.x();
        orientation["y"] = quaternion.y();
        orientation["z"] = quaternion.z();
        request["dest_orientation"] = orientation;
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        OPERATE_LOG(QString("SLAM：开始导航(%1,%2),角度(%3°)").arg(dest.x()).arg(dest.y()).arg(angle));
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        return written == data.size();
    }
    else
        return false;
}

bool Robot::stopNavigatione()
{
    if(m_sockSlamUDP)
    {
        QJsonObject request;
        request["cmd"] = "stop_navigatione";
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockSlamUDP->writeDatagram(data,QHostAddress(m_strSlamIP), m_nSlamPort);
        OPERATE_LOG("SLAM：停止导航");
        return written == data.size();
    }
    else
        return false;
}

bool Robot::startTelemetry()
{
    if(m_sockTelemetryWS)
    {
        QJsonObject request;
        request["cmd"] = "START_TELEMETRY";
        request["mid"] = ++m_nTelemetryMid;
        m_mpTelemetryCmd[m_nTelemetryMid]= "START_TELEMETRY";
        if(m_nTelemetryMid == 255)
            m_nTelemetryMid = 0;
        request["type"] = "TelemetryPayload";
        QJsonObject data;
        data["temp_folder"] = "/tmp/ros_bags";
        data["data_folder"] =  SysSettingModel::getInstance()->strTelemetryFolder();
        data["equipment"] = "";
        data["fps"] = 30;
        request["data"] = data;
        QByteArray str = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockTelemetryWS->sendTextMessage(str);

        OPERATE_LOG("数采：开始数采");
        return written == str.size();
    }
    return false;
}

bool Robot::stopTelemetry()
{
    if(m_sockTelemetryWS)
    {
        QJsonObject request;
        request["cmd"] = "STOP_TELEMETRY";
        request["mid"] = ++m_nTelemetryMid;
        m_mpTelemetryCmd[m_nTelemetryMid]= "STOP_TELEMETRY";
        if(m_nTelemetryMid == 255)
            m_nTelemetryMid = 0;
        request["type"] = "EmptyPayload";
        QJsonObject data;
        request["data"] = data;
        QByteArray str = QJsonDocument(request).toJson(QJsonDocument::Compact);
        qint64 written = m_sockTelemetryWS->sendTextMessage(str);
        OPERATE_LOG("数采：停止数采");
        return written == str.size();
    }
    return false;
}

bool Robot::zeroJoints(QVector<int> lsJointIndex)
{
    quint64 joints = 0;
    for(int i = 0; i < lsJointIndex.size(); ++i)
    {
        joints |= (quint64(1) << lsJointIndex[i]);
    }
    QByteArray arr;
    QDataStream stream(&arr, QIODevice::WriteOnly);
    stream.setByteOrder(QDataStream::LittleEndian);
    stream <<joints;
    //要求发送5次，间隔50ms
    sendZeroJoint(arr);
    for(int i = 1; i < 5; ++i)
    {
        QTimer::singleShot(i*50,[=](){
            sendZeroJoint(arr);});
    }
    QString str;
    for(int i = 0; i < lsJointIndex.size(); ++i)
    {
        str +=QString("%1 ").arg(lsJointIndex[i]);
    }
    OPERATE_LOG("标零：关节("+str +")");
    return true;
}


bool Robot::setCmd(int nCmd)
{
    m_SportCmd.cmdType[1] = nCmd;
    if(m_sockPerceptionTCP)
    {
        QJsonObject dataInfo;
        dataInfo["data_type"] = "command_robotstate";
        dataInfo["value"] = nCmd;

        QJsonObject request;
        request["data_info"] = dataInfo;
        QByteArray data = QJsonDocument(request).toJson(QJsonDocument::Compact);
        m_sockPerceptionTCP->write(data);
        //return written == data.size();
    }
    return true;
}

int Robot::getNStage() const
{
    return m_nStage;
}

QString Robot::strPassword() const
{
    return m_strPassword;
}

QString Robot::strUser() const
{
    return m_strUser;
}

HeadType Robot::headType() const
{
    return m_headType;
}

int Robot::nSpeakPort() const
{
    return m_nSpeakPort;
}

QString Robot::strSpeakIP() const
{
    return m_strSpeakIP;
}

int Robot::nSlamPort() const
{
    return m_nSlamPort;
}

QString Robot::strSlamIP() const
{
    return m_strSlamIP;
}

int Robot::nTelemetryPort() const
{
    return m_nTelemetryPort;
}

QString Robot::strTelemetryIP() const
{
    return m_strTelemetryIP;
}

int Robot::nZeroPort() const
{
    return m_nZeroPort;
}

QString Robot::strZeroIP() const
{
    return m_strZeroIP;
}

int Robot::nSportPort() const
{
    return m_nSportPort;
}

QString Robot::strPerceptionIP() const
{
    return m_strPerceptionIP;
}

int Robot::nPerceptionrPort() const
{
    return m_nPerceptionrPort;
}

QString Robot::strSportIP() const
{
    return m_strSportIP;
}

int Robot::nAudioFileID() const
{
    return m_nAudioFileID;
}

QVector<float> Robot::rightFingerSpeedExp() const
{
    return m_rightFingerSpeedExp;
}

QVector<float> Robot::rightFingerSpeed() const
{
    return m_rightFingerSpeed;
}

QVector<float> Robot::rightFingerMomentExp() const
{
    return m_rightFingerMomentExp;
}

QVector<float> Robot::rightFingerMoment() const
{
    return m_rightFingerMoment;
}

QVector<float> Robot::rightFingerArcExp() const
{
    return m_rightFingerArcExp;
}

QVector<float> Robot::rightFingerArc() const
{
    return m_rightFingerArc;
}

QVector<float> Robot::leftFingerSpeedExp() const
{
    return m_leftFingerSpeedExp;
}

QVector<float> Robot::leftFingerSpeed() const
{
    return m_leftFingerSpeed;
}

QVector<float> Robot::leftFingerMomentExp() const
{
    return m_leftFingerMomentExp;
}

QVector<float> Robot::leftFingerMoment() const
{
    return m_leftFingerMoment;
}

QVector<float> Robot::leftFingerArcExp() const
{
    return m_leftFingerArcExp;
}

QVector<float> Robot::leftFingerArc() const
{
    return m_leftFingerArc;
}

int Robot::nGlobalCmdID() const
{
    return m_nGlobalCmdID;
}

void Robot::setPage(TabType page)
{
    // QString strTab;
    // switch (page) {
    // case MANUAL_PAGE:
    //     setCmd(221);
    //     QTimer::singleShot(1000,[this](){
    //         setCmd(116);
    //     });
    //     QTimer::singleShot(2000,[this](){
    //         setCmd(153);
    //     });
    //     QTimer::singleShot(3000,[this](){
    //         setCmd(152);
    //     });
    //     break;
    // case TEACHING_PAGE:
    //     setCmd(221);
    //     QTimer::singleShot(1000,[this](){
    //         setCmd(116);
    //     });
    //     QTimer::singleShot(2000,[this](){
    //         setCmd(153);
    //     });
    //     break;
    // case REMOTE_OPERATION_PAGE:
    //     setCmd(221);
    //     QTimer::singleShot(1000,[this](){
    //         setCmd(116);
    //     });
    //     break;
    // case AUTO_PAGE:
    // case SET_PAGE:
    //     break;
    // default:
    //     break;
    // }
    m_nCurPage = page;
}

void Robot::setTeleoperation(int nTeleoperation)
{
    switch (nTeleoperation) {
    case STATE_VISIONPRO:
        setCmd(154);
        break;
    case STATE_XSENSE:
        setCmd(155);
        break;
    case STATE_QUEST:

        break;
    default:
        break;
    }
}

void Robot::startTeleoperation()
{
    setCmd(152);
}

void Robot::stopTeleoperation()
{
    setCmd(151);
}

bool Robot::OpenSpeaker()
{
    QString strErr;
    connectSpeakWS(strErr);
    if(m_pAudioInputDevice == NULL)
    {
        QAudioFormat format;
        format.setSampleSize(16);
        format.setChannelCount(1);
        format.setCodec("audio/pcm");
        format.setSampleType(QAudioFormat::SignedInt);
        format.setByteOrder(QAudioFormat::LittleEndian);
        format.setSampleRate(16000);
        m_pAudioInput = new QAudioInput(format);
        m_pAudioInputDevice = m_pAudioInput->start();
        if(m_pAudioInputDevice == NULL)
        {
            return false;
        }
        else
        {
            connect(m_pAudioInputDevice,&QIODevice::readyRead,this,[=](){
                QByteArray in = m_pAudioInputDevice->readAll();
                if(m_sockSpeakWS && in.length() > 0)
                    m_sockSpeakWS->sendBinaryMessage(in);
            });
            return true;
        }
    }
    OPERATE_LOG("打开喊话");
    return true;
}

bool Robot::closeSpeaker()
{
    m_pAudioInputDevice = NULL;
    if(m_pAudioInput)
    {
        m_pAudioInput->stop();
        delete m_pAudioInput;
        m_pAudioInput = NULL;
    }
    disConnectSpeakWS();
    OPERATE_LOG("停止喊话");
    return true;
}

QList<QJsonDocument> Robot::parseJsonFromBuffer(QByteArray &buffer)
{
    QList<QJsonDocument> results;

    while (!buffer.isEmpty()) {
        // 查找完整的JSON对象
        int startIndex = buffer.indexOf('{');
        if (startIndex == -1) {
            buffer.clear();
            break;
        }

        int depth = 0;
        int endIndex = -1;

        // 从起始位置开始扫描，找到匹配的结束括号
        for (int i = startIndex; i < buffer.size(); ++i) {
            if (buffer[i] == '{') {
                depth++;
            } else if (buffer[i] == '}') {
                depth--;
                if (depth == 0) {
                    endIndex = i;
                    break;
                }
            }
        }

        // 如果找到完整的JSON对象
        if (endIndex != -1) {
            // 提取JSON字符串
            QByteArray jsonData = buffer.mid(startIndex, endIndex - startIndex + 1);

            // 解析JSON
            QJsonParseError error;
            QJsonDocument doc = QJsonDocument::fromJson(jsonData, &error);

            if (error.error == QJsonParseError::NoError) {
                // 解析成功，添加到结果列表
                results.append(doc);

                // 从缓冲区移除已处理的数据
                buffer.remove(0, endIndex + 1);
            } else {
                // JSON解析失败，移除损坏的数据
                buffer.remove(0, startIndex + 1);
            }
        } else {
            // 没有找到完整的JSON对象，等待更多数据
            break;
        }
    }

    return results;
}



void Robot::processPerception(const QJsonDocument& jsonDoc)
{
    QJsonObject obj = jsonDoc.object();
    QJsonObject dataInfo = obj["data_info"].toObject();
    QString dataType = dataInfo["data_type"].toString();
    try {
        if (dataType == "respond_control_state") {
            bool value = dataInfo["value"].toBool();
            if(value)
            {
                RobotList::getInstance()->setCurRobot(this);
                QString strErr;
                if(!connectPerceptionUDP(strErr) || !connectSportUDP(strErr))
                {
                    emit errorOccurred(strErr);
                    disconnectRobot();
                }
            }
            else
            {
                emit errorOccurred("感知拒绝连接！");
                disconnectRobot();
            }
        }
        else if (dataType == "joint_info") {
            m_leftTip.fill(0,6);
            m_rightTip.fill(0,6);
            m_leftTipExp.fill(0,6);
            m_rightTipExp.fill(0,6);

            m_leftFingerArc.fill(0,6);
            m_leftFingerArcExp.fill(0,6);
            m_leftFingerMoment.fill(0,6);
            m_leftFingerMomentExp.fill(0,6);
            m_leftFingerSpeed.fill(0,6);
            m_leftFingerSpeedExp.fill(0,6);

            m_rightFingerArc.fill(0,6);
            m_rightFingerArcExp.fill(0,6);
            m_rightFingerMoment.fill(0,6);
            m_rightFingerMomentExp.fill(0,6);
            m_rightFingerSpeed.fill(0,6);
            m_rightFingerSpeedExp.fill(0,6);
            QVector<JOINT> vtLeftHandJoint = getJointByType(LEFT_TOOL_HAND_JOINT);
            for(int i = 0; i < vtLeftHandJoint.size(); ++i)
            {
                QJsonArray leftFingers = jsonDoc[vtLeftHandJoint[i].strProtocal].toArray();
                if(leftFingers.size() >= 6)
                {
                    m_leftFingerArc[vtLeftHandJoint[i].nIndex] = leftFingers[0].toDouble();
                    m_leftFingerArcExp[vtLeftHandJoint[i].nIndex] = leftFingers[1].toDouble();
                    m_leftFingerMoment[vtLeftHandJoint[i].nIndex] = leftFingers[2].toDouble();
                    m_leftFingerMomentExp[vtLeftHandJoint[i].nIndex] = leftFingers[3].toDouble();
                    m_leftFingerSpeed[vtLeftHandJoint[i].nIndex] = leftFingers[4].toDouble();
                    m_leftFingerSpeedExp[vtLeftHandJoint[i].nIndex] = leftFingers[5].toDouble();
                }
            }

            QVector<JOINT> vtRightHandJoint = getJointByType(RIGHT_TOOL_HAND_JOINT);
            for(int i = 0; i < vtRightHandJoint.size(); ++i)
            {
                QJsonArray rightFingers = jsonDoc[vtRightHandJoint[i].strProtocal].toArray();
                if(rightFingers.size() >= 6)
                {
                    m_rightFingerArc[vtRightHandJoint[i].nIndex] = rightFingers[0].toDouble();
                    m_rightFingerArcExp[vtRightHandJoint[i].nIndex] = rightFingers[1].toDouble();
                    m_rightFingerMoment[vtRightHandJoint[i].nIndex] = rightFingers[2].toDouble();
                    m_rightFingerMomentExp[vtRightHandJoint[i].nIndex] = rightFingers[3].toDouble();
                    m_rightFingerSpeed[vtRightHandJoint[i].nIndex] = rightFingers[4].toDouble();
                    m_rightFingerSpeedExp[vtRightHandJoint[i].nIndex] = rightFingers[5].toDouble();
                }
            }
            QJsonArray leftArm = jsonDoc["ee_pose_l"].toArray();
            for(int i = 0; i < leftArm.size() && i < 6; ++i)
            {
                m_leftTip[i] = leftArm[i].toDouble();
            }
            QJsonArray rightArm = jsonDoc["ee_pose_r"].toArray();
            for(int i = 0; i < rightArm.size() && i < 6; ++i)
            {
                m_rightTip[i] = rightArm[i].toDouble();
            }
            QJsonArray leftArmExp = jsonDoc["ee_pose_l_exp"].toArray();
            for(int i = 0; i < leftArmExp.size() && i < 6; ++i)
            {
                m_leftTipExp[i] = leftArmExp[i].toDouble();
            }
            QJsonArray rightArmExp = jsonDoc["ee_pose_r_exp"].toArray();
            for(int i = 0; i < rightArmExp.size() && i < 6; ++i)
            {
                m_rightTipExp[i] = rightArmExp[i].toDouble();
            }
            emit toolStateChanged();
        }
        else if(dataType == "guard_info"){
            QString value = dataInfo["value"].toString();
            if(value == "sound")
            {
                emit sentrySoundChanged(obj["microphone_array"].toInt());
            }
            else if(value == "laser")
            {
                int nStatus = obj["status"].toInt();
                int nDestNum = (nStatus & 0xFF00)>>16;
                int nDest = nStatus&0xFF;
                float fDistance =  obj["distance"].toInt();
                emit sentryDistanceChanged(nDest,nDestNum,fDistance);
            }
            else if(value == "optical")
            {
                float fieldangle = obj["fieldangle"].toDouble();
                float undershootX  = obj["undershoot_X"].toDouble();
                float undershootY = obj["undershoot_Y"].toDouble();
                int nDestCount = obj["target_count"].toInt();
                QVector<SentryDestInfo> vtDest;
                for(int i = 0; i < nDestCount; ++i)
                {
                    QJsonArray dest = obj[QString("targetinfo_%1").arg(i+1)].toArray();
                    SentryDestInfo destInfo;
                    if(dest.size() >= 5)
                    {
                        destInfo.nX = dest[0].toInt();
                        destInfo.nY = dest[1].toInt();
                        destInfo.nW = dest[2].toInt();
                        destInfo.nH = dest[3].toInt();
                        destInfo.fDegree = dest[4].toDouble();
                        vtDest.push_back(destInfo);
                    }
                }
                emit sentryIndifyChanged(fieldangle,undershootX,undershootY,vtDest);
            }
        }
    }
    catch (const std::exception& e) {
        Q_UNUSED(e);
    }
}

void Robot::processSlam(const QJsonDocument &jsonDoc)
{
    QJsonObject obj = jsonDoc.object();
    QString strCmd = obj["cmd"].toString();
    if(strCmd == "trans_map")
    {
        QByteArray pgmData = QByteArray::fromBase64(obj["pgm"].toString().toLocal8Bit());

        int width = 0, height = 0, maxValue = 0;
        int pos = 0;

        // 跳过文件类型（P5）
        while (pos < pgmData.size() && pgmData[pos] != '\n') ++pos;
        ++pos;

        // 跳过注释行
        while (pos < pgmData.size() && pgmData[pos] == '#') {
            while (pos < pgmData.size() && pgmData[pos] != '\n') ++pos;
            ++pos;
        }

        // 读取宽度和高度
        sscanf(pgmData.constData() + pos, "%d %d", &width, &height);
        while (pos < pgmData.size() && pgmData[pos] != '\n') ++pos;
        ++pos;

        // 读取最大灰度值
        sscanf(pgmData.constData() + pos, "%d", &maxValue);
        while (pos < pgmData.size() && pgmData[pos] != '\n') ++pos;
        ++pos;

        // 创建QImage对象
        QImage image(width, height, QImage::Format_Grayscale8);

        // 复制像素数据
        const uchar* pixelData = reinterpret_cast<const uchar*>(pgmData.constData() + pos);
        for (int y = 0; y < height; ++y) {
            uchar* scanLine = image.scanLine(y);
            for (int x = 0; x < width; ++x) {
                scanLine[x] = pixelData[y * width + x];
            }
        }
        m_imgMap = image;
        QJsonArray arr = obj["origin"].toArray();
        m_ptMapOrigin = QPointF(arr[0].toDouble(),arr[1].toDouble());
        m_nMapRes = obj["resolution"].toDouble();
        emit mapChanged();
        m_reGetMapTimer.stop();
    }
    else if(strCmd == "update_position")
    {
        QJsonObject position = obj["position"].toObject();
        m_ptMapRobot.setX(position["x"].toDouble());
        m_ptMapRobot.setY(position["y"].toDouble());

        QJsonObject orientation = obj["orientation"].toObject();

        QQuaternion  quaternion(orientation["w"].toDouble(),orientation["x"].toDouble(),orientation["y"].toDouble(),orientation["z"].toDouble());

        QVector3D eulerAngles = quaternion.toEulerAngles();
        m_fMapRobotAngle = eulerAngles.z();

        emit robotPositionChanged();
    }
    else if(strCmd == "send_pointcloud")
    {
        //基于现在的机器人位置和角度，生成一张雷达图
        if(!m_imgMap.isNull())
        {
            float angle_min = obj["angle_min"].toDouble();
            //float angle_max = obj["angle_max"].toDouble();
            float angle_increment = obj["angle_increment"].toDouble();
            float range_min = obj["range_min"].toDouble();
            float range_max = obj["range_max"].toDouble();
            QString ranges = obj["ranges"].toString();
            QStringList lsRange = ranges.split(",");
            // 创建QImage对象
            QImage image(range_max/m_nMapRes*2, range_max/m_nMapRes*2, QImage::Format_ARGB32);
            image.fill(Qt::transparent);

            QPainter painter(&image);
            painter.setPen(Qt::NoPen);
            painter.setBrush(QColor(255, 0, 0, 200));

            float angle = angle_min;
            for (const QString &rangeStr : lsRange) {
                float range = rangeStr.toFloat();
                if (range >= range_min && range <= range_max) {
                    float x = (range_max + range * sin(-angle + m_fMapRobotAngle/180*M_PI))/m_nMapRes;
                    float y = (range_max - range * cos(-angle + m_fMapRobotAngle/180*M_PI))/m_nMapRes;
                    QPointF point(x , y );
                    painter.drawEllipse(point, 2, 2);
                }
                angle += angle_increment;
            }
            m_imgPointCloud = image;
            m_ptPointCloud = QPointF(m_ptMapRobot.x() - range_max,m_ptMapRobot.y()+range_max);
            emit pointCloudChanged();
        }
    }
}

void Robot::onStickKeyPressed(FUNCTION_KEY nFuncKey, STICK_KEY nKey)
{
    //设置界面不发送数据
    if(m_nCurPage == SET_PAGE)
        return;
    //LB
    if(nKey == STICK_KEY5)
    {
        m_nStage++;
        emit stageChanged();
    }
    else if(nKey == STICK_KEY6)
    {
        m_nStage = (m_nStage>0?(m_nStage-1):0);
        emit stageChanged();
    }
    else if(nKey==STICK_KEY3){
        m_nStage =0;
        emit stageChanged();
    }


    Command cmd;
    CMD_TYPE type = SysSettingModel::getInstance()->getCmdByHotKey(nFuncKey,nKey,cmd);
    qDebug()<<"cmdtype:"<<type<<"cmd:"<<cmd.strShowText;
    switch (type) {
    case CMD_TYPE_UPPER:
        setUpperCmd(cmd);
        break;
    case CMD_TYPE_GLOBAL:
        if(cmd.strShowText.trimmed() == "上使能")
        {
            setEnable(true);
        }
        else if(cmd.strShowText.trimmed() == "下使能")
        {
            setEnable(false);
        }
        else
        {
            setGlobalCmd(cmd);
        }
        break;
    default:
        break;
    }

    AudioFile file;
    if(SysSettingModel::getInstance()->getAudioByHotKey(nFuncKey,nKey,file))
    {
        setAudioFlie(file);
    }

}

void Robot::onStickXChanged(float x)
{
    if(m_bJoystickState){
        m_SportCmd.turnSpeed = -x * 100;
    }else{
        m_SportCmd.xSpeed = x * 100;
    }
}

void Robot::onStickYChanged(float y)
{
    m_SportCmd.ySpeed = y * 100;
}

void Robot::onSticUChanged(float u)
{
    if(m_bJoystickState){
        m_SportCmd.xSpeed = u * 100;
    }else{
        m_SportCmd.turnSpeed = -u * 100;
    }
}

void Robot::onStickRChanged(float r)
{
    Q_UNUSED(r);
}

void Robot::onStickZChanged(float z)
{
    if(z < 0)
    {
        m_SportCmd.rightT = -z * 100;
        m_SportCmd.leftT = 0;
    }
    else if(z > 0)
    {
        m_SportCmd.leftT = z * 100;
        m_SportCmd.rightT = 0;
    }
    else
        m_SportCmd.leftT = m_SportCmd.rightT = 0;
}

void Robot::checkWarning(SportState oldState, SportState newState)
{
    if(/*memcmp(oldState.state,newState.state,sizeof(oldState.state))!= 0
                ||*/memcmp(oldState.error,newState.error,sizeof(oldState.error))!= 0
            ||memcmp(oldState.szWarning,newState.szWarning,sizeof(oldState.szWarning))!= 0
            ||memcmp(oldState.tempture,newState.tempture,sizeof(oldState.tempture))!= 0)
    {
        m_strWarning = "";
        //头部
        checkWarningByType(HEAD_JOINT,oldState,newState);
        //腰部
        checkWarningByType(WAIST_JOINT,oldState,newState);
        //左臂
        checkWarningByType(LEFT_ARM_JOINT,oldState,newState);
        //右臂
        checkWarningByType(RIGHT_ARM_JOINT,oldState,newState);
        //左腿
        checkWarningByType(LEFT_LEG_JOINT,oldState,newState);
        //右腿
        checkWarningByType(RIGHT_LEG_JOINT,oldState,newState);
        emit warningChanged(m_strWarning);
        emit tempratureTooHighChanged(m_lsJointTooHigh);
    }
}

void Robot::checkWarningByType(JointType type, SportState oldState, SportState newState)
{
    QVector<JOINT> jointList = getJointByType(type);
    QString typeName = getJointTypeText(type);
    for(int i = 0; i < jointList.size(); ++i)
    {
        if(jointList[i].nIndex < 31)
        {
            QString strState="";//状态字

            if(int(newState.state[jointList[i].nIndex])  == 536 )
            {
                strState = QString("状态字(0x%1)-错误").arg(int(newState.state[jointList[i].nIndex]),4,16,QChar('0'));
                //switch()
                if(oldState.state[jointList[i].nIndex] != newState.state[jointList[i].nIndex])
                    WarningLogModel::getInstance()->addData(typeName + jointList[i].strName+"："+strState);
            }
            QString strErr = "";//错误码
            if(int(newState.error[jointList[i].nIndex]) > 0)
            {
                strErr = QString("错误码(0x%1)-").arg(int(newState.error[jointList[i].nIndex]),4,16,QChar('0'));
                switch (int(newState.error[jointList[i].nIndex])) {
                case 0x2311: strErr += "过流"; break;
                case 0x2340: strErr += "短路(电机接线或其他异常、驱动器故障)"; break;
                case 0x3120: strErr += "⽋压"; break;
                case 0x3130: strErr += "相位丢失"; break;
                case 0x3310: strErr += "过压"; break;
                case 0x4310: strErr += "驱动器过热"; break;
                case 0x7121: strErr += "电机卡住"; break;
                case 0x7300: strErr += "反馈错误"; break;
                case 0x8130: strErr += "⼼跳事件"; break;
                case 0x8140: strErr += "从总线断开恢复"; break;
                case 0x8311: strErr += "超过峰值电流"; break;
                case 0x8480: strErr += "速度跟踪错误"; break;
                case 0x8611: strErr += "位置跟踪错误"; break;
                case 0xFF10: strErr += "不能启动电机"; break;
                case 0xFF20: strErr += "安全模式打开"; break;
                default: strErr += "未定义";  break;
                }
                if(oldState.error[jointList[i].nIndex] != newState.error[jointList[i].nIndex] && strErr.length() > 0)
                    WarningLogModel::getInstance()->addData(typeName + jointList[i].strName+"："+strErr);
            }

            QString strWarning="";//告警

            if(int(newState.szWarning[jointList[i].nIndex]) > 0)
            {
                strWarning = QString("告警码(%1)-").arg(newState.szWarning[jointList[i].nIndex]);
                switch(int(newState.szWarning[jointList[i].nIndex]))
                {
                case 1: strWarning += "电机位置异常"; break;
                case 2: strWarning += "电机电流异常"; break;
                case 3: strWarning += "电机温度异常"; break;
                default: strWarning += "未定义";  break;
                }
                if(oldState.szWarning[jointList[i].nIndex] != newState.szWarning[jointList[i].nIndex] &&strWarning.length() > 0)
                    WarningLogModel::getInstance()->addData(typeName + jointList[i].strName+"："+strWarning);
            }
            QString strTemprature="";//温度过高
            if(int(newState.tempture[jointList[i].nIndex]) > 70){
                strTemprature+=typeName+jointList[i].strName+"温度超过70°";
                if(!m_lsJointTooHigh.contains(strTemprature)){
                    m_lsJointTooHigh.append(strTemprature);
                    //                    WarningLogModel::getInstance()->addData(typeName + jointList[i].strName+"："+strTemprature);
                }
            }else if(int(newState.tempture[jointList[i].nIndex]) <=70){
                strTemprature+=typeName+jointList[i].strName+"温度超过70°";
                if(m_lsJointTooHigh.contains(strTemprature)){
                    m_lsJointTooHigh.removeAll(strTemprature);
                }
            }


            QString str = strState;
            if(strErr.length() > 0)
            {
                if(str.length() > 0)
                    str = str + "、" + strErr;
                else
                    str = strErr;
            }
            if(strWarning.length() > 0)
            {
                if(str.length() > 0)
                    str = str + "、" + strWarning;
                else
                    str = strWarning;
            }
            //            if(strTemprature.length() > 0)
            //            {
            //                if(str.length() > 0)
            //                    str = str + "、" + strTemprature;
            //                else
            //                    str = strTemprature;
            //            }
            if(str.length())
            {
                if(m_strWarning.length() > 0)
                    m_strWarning = m_strWarning +";    " + typeName + jointList[i].strName+"："+ str;
                else
                    m_strWarning = (typeName + jointList[i].strName+"：" + str);
            }
        }
    }
}

void Robot::onReGetMapTimer()
{
    getMap();
}

bool Robot::sendZeroJoint(QByteArray arrData)
{
    if(m_sockZeroUDP)
    {
        //时间戳
        qint64 timestamp = QDateTime::currentMSecsSinceEpoch();
        QByteArray cmd;
        QDataStream stream(&cmd, QIODevice::WriteOnly);
        stream.setByteOrder(QDataStream::LittleEndian);
        stream <<(quint16)Portocal_Mask_Controller
              <<(quint16)(arrData.length() + 2)
             <<(quint32)((Protocal_DataType_Zero<<8)|(Protocal_Type_Controller<<24))
            <<(qint64)timestamp;
        cmd.append(arrData);
        unsigned char check = 0;
        for(int i = 0; i < cmd.length(); i++)
        {
            check = check ^cmd[i];
        }
        cmd.append(check);
        return cmd.length() == m_sockZeroUDP->writeDatagram(cmd,QHostAddress(m_strZeroIP), m_nZeroPort);
    }
    return false;
}

QPointF Robot::ptPointCloud() const
{
    return m_ptPointCloud;
}

QImage *Robot::pImgPointCloud()
{
    return &m_imgPointCloud;
}

QImage * Robot::pImgMap()
{
    return &m_imgMap;
}

float Robot::mapRes() const
{
    return m_nMapRes;
}

QPointF Robot::mapOrigin() const
{
    return m_ptMapOrigin;
}

QPointF Robot::mapRobot() const
{
    return m_ptMapRobot;
}

float Robot::mapRobotAngle() const
{
    return m_fMapRobotAngle;
}


void Robot::setLsWnd(const QList<int> &newLsWnd)
{
    DatabaseManager::getInstance()->setLsWnd(m_nID, newLsWnd);
}

//显示窗口,视频为在m_lsVideo中的下标+1，1001--rudf    1002---感知
QList<int> Robot::lsWnd() const
{
    return DatabaseManager::getInstance()->getLsWnd(m_nID);
}

int Robot::nUpperCmdID() const
{
    return m_nUpperCmdID;
}

SportState Robot::SportStateData()
{
    return m_SportState;
}


QVector<float> Robot::rightTip() const
{
    return m_rightTip;
}

QVector<float> Robot::leftTip() const
{
    return m_leftTip;
}

ToolType Robot::rightTool() const
{
    return m_rightTool;
}

QVector<float> Robot::getJointsArc(JointType type)
{
    QVector<float> JointsArc;
    float * realArc=m_SportState.realArc;
    QVector<JOINT> joints= getJointByType(type);
    for(int i=0;i<joints.size();i++)
    {
        JointsArc.push_back(realArc[joints[i].nIndex]);
    }
    return JointsArc;
}

ToolType Robot::leftTool() const
{
    return m_leftTool;
}

int Robot::nID() const
{
    return m_nID;
}

QString Robot::strName() const
{
    return m_strName;
}


QList<VideoInfo> Robot::lsVideo() const
{
    return m_lsVideo;
}

QString Robot::strWarning() const
{
    return m_strWarning;
}

void Robot::setVoiceCmdText(QString strText)
{
    // //处理语音指令
    // RobotType * pRobotType = getRobotType();
    // if(pRobotType)
    // {
    //     QVariantMap mpConfig = pRobotType->mpConfig();
    //     QVariantList ls = mpConfig[configConstants::GAIT].toList();
    //     for (int i = 0; i < ls.count(); ++i) {
    //         QString str=ls[i].toMap()[configConstants::DISPLAY].toString();
    //         if(strText.contains(str))
    //         {
    //             QString strCmd=ls[i].toMap()[configConstants::PROTOCAL_KEY].toString();
    //             OPERATE_LOG("执行语音命令："+strCmd);
    //             setGait(strCmd);
    //             return;
    //         }
    //     }

    //     ls = mpConfig[configConstants::ACTION].toList();
    //     for (int i = 0; i < ls.count(); ++i) {
    //         QString str=ls[i].toMap()[configConstants::DISPLAY].toString();
    //         if(strText.contains(str))
    //         {
    //             QString strCmd=ls[i].toMap()[configConstants::PROTOCAL_KEY].toString();
    //             OPERATE_LOG("执行语音命令："+strCmd);
    //             setAction(strCmd);
    //             return;
    //         }
    //     }
    // }
}

void Robot::onPerceptionTCPReadyRead()
{
    QByteArray arr = m_sockPerceptionTCP->readAll();
    m_tcpPerceptionBuffer.append(arr);
    // 解析缓冲区中的所有完整JSON对象
    QList<QJsonDocument> jsonDocs = parseJsonFromBuffer(m_tcpPerceptionBuffer);
    // 处理所有解析出的JSON对象
    for (const QJsonDocument &doc : jsonDocs) {
        processPerception(doc);
    }
}

void Robot::onPerceptionUDPReadyRead()
{
    while (m_sockPerceptionUDP->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(m_sockPerceptionUDP->pendingDatagramSize());
        QHostAddress sender;
        quint16 senderPort;

        m_sockPerceptionUDP->readDatagram(datagram.data(), datagram.size(),
                                          &sender, &senderPort);
        QJsonDocument doc = QJsonDocument::fromJson(datagram);
        if (!doc.isNull()) {
            processPerception(doc);
        }
    }
}

void Robot::onSportUDPReadyRead()
{
    while (m_sockSportUDP->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(m_sockSportUDP->pendingDatagramSize());
        QHostAddress sender;
        quint16 senderPort;

        m_sockSportUDP->readDatagram(datagram.data(), datagram.size(),
                                     &sender, &senderPort);
        QString s = sender.toString();
        if(sender.isEqual(QHostAddress(m_strSportIP)) && datagram.size() == sizeof(SportState))
        {
            if(!m_bRecvHex)
            {
                m_bRecvHex = true;
                QString strErr;
                if(hasSlam())
                {
                    if(!connectSlamUDP(strErr))
                        emit errorOccurred(strErr);
                }
                connectZeroUDP(strErr);
                m_nStage = 0;
                emit stageChanged();
                emit controlStateChanged(true);
                connect(StickControl::getInstance(),&StickControl::keyPressed,this,&Robot::onStickKeyPressed,Qt::UniqueConnection);
                connect(StickControl::getInstance(),&StickControl::xChanged,this,&Robot::onStickXChanged,Qt::UniqueConnection);
                connect(StickControl::getInstance(),&StickControl::yChanged,this,&Robot::onStickYChanged,Qt::UniqueConnection);
                connect(StickControl::getInstance(),&StickControl::uChanged,this,&Robot::onSticUChanged,Qt::UniqueConnection);
                connect(StickControl::getInstance(),&StickControl::rChanged,this,&Robot::onStickRChanged,Qt::UniqueConnection);
                connect(StickControl::getInstance(),&StickControl::zChanged,this,&Robot::onStickZChanged,Qt::UniqueConnection);
            }
            SportState SportState;
            memcpy(&SportState, datagram.data(),datagram.size());
            checkWarning(m_SportState,SportState);
            m_SportState= SportState;
            emit sportStateChanged();
            m_sportRecvTimer.start(5000);
        }
    }
}

void Robot::onSlamUDPReadyRead()
{
    //发过来的包是分开的，需要合并
    while(m_sockSlamUDP->hasPendingDatagrams())
    {
        QByteArray datagram;
        datagram.resize(m_sockSlamUDP->pendingDatagramSize());
        QHostAddress address;
        quint16 port;
        m_sockSlamUDP->readDatagram(datagram.data(), datagram.size(),&address,&port);
        if(datagram.length() > 10)
        {
            //判断校验
            unsigned char szSum = 0x00;
            for(int i = 0; i < datagram.length()-1; ++i)
            {
                szSum = (szSum+datagram.data()[i])&0xFF;
            }
            if(szSum == (unsigned char)datagram.data()[datagram.length()-1])
            {
                Slam_msg * msg = (Slam_msg*)datagram.data();
                msg->nPkCount = qFromLittleEndian(msg->nPkCount);
                msg->nPkIndex = qFromLittleEndian(msg->nPkIndex);
                msg->nLen = qFromLittleEndian(msg->nLen);
                QByteArray data((const char *)msg->szData,msg->nLen);

                if(!m_mpSlamRecv.contains(msg->szIndex))
                {
                    SLAM_PACKAGES pks;
                    m_mpSlamRecv.insert(msg->szIndex,pks);
                }
                m_mpSlamRecv[msg->szIndex].nPkCount = msg->nPkCount;
                m_mpSlamRecv[msg->szIndex].dataList.insert(msg->nPkIndex,data);
                if(msg->nPkCount == m_mpSlamRecv[msg->szIndex].dataList.size())
                {
                    QString strData = "";
                    for(QMap<int,QString>::Iterator it = m_mpSlamRecv[msg->szIndex].dataList.begin(); it != m_mpSlamRecv[msg->szIndex].dataList.end(); ++it)
                    {
                        strData += it.value();
                    }
                    m_mpSlamRecv[msg->szIndex].dataList.clear();
                    QJsonDocument doc = QJsonDocument::fromJson(strData.toUtf8());
                    if (!doc.isNull()) {
                        processSlam(doc);
                    }
                }
            }
        }
    }
}

void Robot::onTelemetryWSReceived(const QString &message)
{
    QJsonDocument doc = QJsonDocument::fromJson(message.toUtf8());
    if (!doc.isNull()) {
        QJsonObject obj = doc.object();
        QString strCmd = obj["cmd"].toString();
        if(strCmd == "ACK")
        {
            if(m_mpTelemetryCmd[obj["mid"].toInt()] == "START_TELEMETRY")
                emit telemetryChanged(true);
            else if(m_mpTelemetryCmd[obj["mid"].toInt()] == "STOP_TELEMETRY")
                emit telemetryChanged(false);
        }
        else if(strCmd == "ERROR")
        {
            QString errMsg = "";
            QJsonObject data = obj["data"].toObject();
            if(m_mpTelemetryCmd[obj["mid"].toInt()] == "START_TELEMETRY")
                errMsg = "开始数采失败：" + data["message"].toString();
            else if(m_mpTelemetryCmd[obj["mid"].toInt()] == "STOP_TELEMETRY")
                errMsg = "停止数采失败" + data["message"].toString();
            if(errMsg.length() > 0)
                emit errorOccurred(errMsg);
        }
        else if(strCmd == "SEND_SAMPLER_PROCESS")
        {
            QJsonObject data = obj["data"].toObject();
            QString result;
            for(QString str:data.keys())
            {
                result += str;
                result += ":[";
                QJsonArray array = data[str].toArray();
                QMap<QString,int> processList;
                for(int i = 0; i < array.size(); ++i)
                {
                    QJsonObject obj = array.at(i).toObject();
                    result += obj["name"].toString();
                    result+= ":";
                    result+= QString("%1").arg(obj["count"].toInt());
                    if(i != array.size() - 1)
                        result+= ", ";
                }
                result += "]    ";
            }
            emit  telemetryResult(result);
        }
    }
}

void Robot::onPerceptionTCPError(QAbstractSocket::SocketError error)
{

    qDebug()<<"连接失败";
    Q_UNUSED(error);
    disConnectPerceptionTCP();
    QString errorMsg = "感知:网络连接失败！";
    m_sportRecvTimer.stop();
    emit errorOccurred(errorMsg);
    disconnectRobot();
}

void Robot::onPerceptionTCPConnect()
{
    QString strErr;
    requestControl(true,strErr);
}

void Robot::onSpeakWSError(QAbstractSocket::SocketError error)
{
    Q_UNUSED(error);
    disConnectSpeakWS();
    QString errorMsg = "喊话:网络连接失败！";
    emit errorOccurred(errorMsg);
    emit speakWSError();
}

void Robot::onTelemetryWSError(QAbstractSocket::SocketError error)
{
    Q_UNUSED(error);
    disConnectTelemetryWS();
    QString errorMsg = "数采:网络连接失败！";
    emit errorOccurred(errorMsg);
    emit telemetryWSError();
}

void Robot::onSportSendTimer()
{
    if(!m_sockSportUDP->writeDatagram(reinterpret_cast<char *>(&m_SportCmd),sizeof(SportCommand),QHostAddress(m_strSportIP),m_nSportPort))
        qDebug()<<"发送16进制命令失败!";
}

void Robot::onSportRecvTimer()
{
    if(m_bRecvHex)
    {
        emit errorOccurred("运控：接收数据超时，断开连接！");
        disconnectRobot();
    }
    else
    {
        emit errorOccurred("运控：未收到数据，连接失败！");
        disconnectRobot();
        m_sportRecvTimer.stop();
    }
}

void Robot::update(QString strName,QString strUrdfName,
                   QString strSportIP,
                   int nSportPort,
                   QString strPerceptionIP,
                   int nPerceptionrPort,
                   QString strSlamIP,
                   int nSlamPort,
                   QString strSpeakIP,
                   int nSpeakPort,
                   QString strZeroIP,
                   int nZeroPort,
                   QString strTelemetryIP,
                   int nTelemetryPort,
                   ToolType nLeftTool,
                   ToolType nRightTool,
                   HeadType headType,
                   QString strUser,
                   QString strPwd,
                   QList<VideoInfo> lsVideo)
{
    m_strName = strName;
    m_strUrdfName=strUrdfName;
    m_strSportIP=strSportIP;
    m_nSportPort=nSportPort;
    m_strPerceptionIP=strPerceptionIP;
    m_nPerceptionrPort=nPerceptionrPort;
    m_strSlamIP=strSlamIP;
    m_nSlamPort=nSlamPort;
    m_strSpeakIP=strSpeakIP;
    m_nSpeakPort=nSpeakPort;
    m_strZeroIP=strZeroIP;
    m_nZeroPort=nZeroPort;
    m_strTelemetryIP=strTelemetryIP;
    m_nTelemetryPort=nTelemetryPort;
    m_leftTool = nLeftTool;
    m_rightTool = nRightTool;
    m_headType = headType;
    m_strUser = strUser;
    m_strPassword = strPwd;
    m_lsVideo = lsVideo;
}
